# -*- coding: utf-8 -*-
import sys
import xml.dom.minidom as dom
import vtk
import os
from vtk.util.colors import red, green, blue
from vtk.qt4.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor

from pyFAMOUSO.publisher import PublisherEventChannel
from pyFAMOUSO.event import Event

from my_simulation import MySimulatedCamera, MySimulatedIRSensor, MySystem
from my_abstract_sensor import MySmartAbstractCamera, MySmartAbstractIRSensor
from my_simulation import MySimulatedTemp
from my_abstract_sensor import MySmartAbstractTemp

from MOSAIC_MultiVirtualSensor import Ui_MOSAIC_virtualSensor as Dlg
from PyQt4 import QtCore, QtGui

import logging
import logging.config
import quantities

class QTextEditLoggingHandler(logging.Handler):
    def __init__(self, text_edit):
        logging.Handler.__init__(self)
        self.text_edit = text_edit

    def emit(self, record):
        self.text_edit.appendPlainText(self.format(record))
    
class GUI( QtGui.QDialog, Dlg):
    def __init__(self):
        QtGui.QDialog.__init__(self) 
        self.setupUi(self)
                
        self.connect(self.pushButton_start_0, QtCore.SIGNAL("clicked()"), self.onStart_0) 
        self.connect(self.pushButton_start_1, QtCore.SIGNAL("clicked()"), self.onStart_1) 
        self.connect(self.pushButton_start_2, QtCore.SIGNAL("clicked()"), self.onStart_2)
 
        self.connect(self.pushButton_startSim, QtCore.SIGNAL("clicked()"), self.onStartSim) 
        self.connect(self.pushButton_stop , QtCore.SIGNAL("clicked()"), self.onStop)   

        self.connect(self.pushButton_loadxml_0, QtCore.SIGNAL("clicked()"), self.loadXML_channel_0)             
        self.connect(self.pushButton_loadxml_1, QtCore.SIGNAL("clicked()"), self.loadXML_channel_1) 
        self.connect(self.pushButton_loadxml_2, QtCore.SIGNAL("clicked()"), self.loadXML_channel_2)  

        self.connect(self.checkBox_CamStereo, QtCore.SIGNAL("stateChanged(int)"), self.setStereoMode)
        self.connect(self.pushButton_CamReset, QtCore.SIGNAL("clicked()"), self.resetCamera)
        self.connect(self.pushButton_CamStandard, QtCore.SIGNAL("clicked()"), self.standardCamera)
        
        
   
        self.init_figures()
        
        self.channel_file_0=self.label_xmlfile_channel_0.text()
        self.channel_file_1=self.label_xmlfile_channel_1.text()       
        self.channel_file_2=self.label_xmlfile_channel_2.text()
        self.sensor_file_0=self.label_xmlfile_sensor_0.text()
        self.sensor_file_1=self.label_xmlfile_sensor_1.text()
        self.sensor_file_2=self.label_xmlfile_sensor_2.text()      
                   
        self.channel_file_0='../datasheets/events/'+"Posi_CA_.xml"
        self.sensor_file_0='../datasheets/sensors/'+'CameraPositionSensor.xml'
        self.label_xmlfile_channel_0.setText(os.path.basename(str(self.channel_file_0)))
        self.label_xmlfile_sensor_0.setText(os.path.basename(str(self.sensor_file_0)))
        
        self.channel_file_1='../datasheets/events/'+"Dist_IR_.xml"
        self.sensor_file_1='../datasheets/sensors/'+'IRDistanceSensor.xml'
        self.label_xmlfile_channel_1.setText(os.path.basename(str(self.channel_file_1)))
        self.label_xmlfile_sensor_1.setText(os.path.basename(str(self.sensor_file_1)))
        
        self.channel_file_2='../datasheets/events/'+"Temp____.xml"
        self.sensor_file_2='../datasheets/sensors/'+'TempSensor.xml'
        self.label_xmlfile_channel_2.setText(os.path.basename(str(self.channel_file_2)))
        self.label_xmlfile_sensor_2.setText(os.path.basename(str(self.sensor_file_2)))

    def disablePositionInput(self, index):
        exec("self.lineEdit_Ori_X_%d.setDisabled(True)"%index)
        exec("self.lineEdit_Ori_Y_%d.setDisabled(True)"%index)
        exec("self.lineEdit_Ori_Z_%d.setDisabled(True)"%index)
        exec("self.lineEdit_Pos_X_%d.setDisabled(True)"%index)
        exec("self.lineEdit_Pos_Y_%d.setDisabled(True)"%index)
        exec("self.lineEdit_Pos_Z_%d.setDisabled(True)"%index)

    def enablePositionInput(self, index):
        exec("self.lineEdit_Ori_X_%d.setDisabled(False)"%index)
        exec("self.lineEdit_Ori_Y_%d.setDisabled(False)"%index)
        exec("self.lineEdit_Ori_Z_%d.setDisabled(False)"%index)
        exec("self.lineEdit_Pos_X_%d.setDisabled(False)"%index)
        exec("self.lineEdit_Pos_Y_%d.setDisabled(False)"%index)
        exec("self.lineEdit_Pos_Z_%d.setDisabled(False)"%index)

    def getPositionFromGUI(self, index):
        cmd="position=[float(self.lineEdit_Pos_X_%d.text()),"%index
        cmd=cmd+"float(self.lineEdit_Pos_Y_%d.text()),"%index
        cmd=cmd+"float(self.lineEdit_Pos_Z_%d.text())]"%index
        exec cmd
        return position
    
    def getOrientationFromGUI(self, index):
        cmd="orientation=[float(self.lineEdit_Ori_X_%d.text()),"%index
        cmd=cmd+"float(self.lineEdit_Ori_Y_%d.text()),"%index
        cmd=cmd+"float(self.lineEdit_Ori_Z_%d.text())]"%index
        exec cmd
        return orientation

    def onStartSim(self):
        self.SimTimer = QtCore.QTimer()
        self.connect(self.SimTimer, QtCore.SIGNAL("timeout()"), self.update_sim)
        updateTime = 0.1
        self.SimTimer.start(updateTime)
        
    def update_sim(self):
        position=MySystem.getPosition()        
        self.actRobot.SetPosition(position[0],position[1],0)
        orientation=MySystem.getOrientation()
        self.actRobot.SetOrientation(0,0,orientation)
        self.redraw()

    def onStart_common(self, index):
        exec "aux=self.pushButton_start_%d.isChecked()"%index
        if aux==True:
            exec "self.pushButton_loadxml_%d.setDisabled(True)"%index
            self.disablePositionInput(index)
            exec "self.SASensor_%d.setPosition(self.getPositionFromGUI(%d)*quantities.centimeter)"%(index, index)
            exec "self.SASensor_%d.setOrientation(self.getOrientationFromGUI(%d)*quantities.degree)"%(index, index)
            exec "self.SASensor_%d.initSensorBeam()"%(index)
            exec "self.SASensor_%d.initMeasurement()"%(index)
            exec "self.SASensor_%d.initText()"%(index)  
        else:
            exec "self.pushButton_loadxml_%d.setDisabled(False)"%index
            self.enablePositionInput(index)
            exec "self.SASensor_%d.hideMeasurement()"%index
            exec "self.SASensor_%d.hideSensorBeam()"%index
            exec "self.SASensor_%d.hideText()"%index 
        
    def onStart_0(self):
        if self.pushButton_start_0.isChecked():
            if self.label_xmlfile_channel_0.text()=='...' or \
               self.label_xmlfile_sensor_0.text()==[]:
                logger.error('Please determine an appropriate data sheet! ')
            self.SensorInterface_0=MySimulatedCamera(sensorDataSheet=self.sensor_file_0,
                                                     position=self.getPositionFromGUI(0),
                                                     orientation=self.getOrientationFromGUI(0),
                                                     logger=logger)
            self.SASensor_0=MySmartAbstractCamera(sensor_interface=self.SensorInterface_0,
                                                  outputDataSheet=self.channel_file_0,
                                                  renderer = self.ren,
                                                  appName=[])
            self.SASensor_0.startMeasurements()
            logger.info("SmartAbstractCamera started")
            self.connect(self.SASensor_0,
                         QtCore.SIGNAL("updateGui ( ) "), 
                         self.redraw) 
        self.onStart_common(0)
        
    def onStart_1(self):
        if self.pushButton_start_1.isChecked():
            if self.label_xmlfile_channel_1.text()=='...' or \
               self.label_xmlfile_sensor_1.text()==[]:
                logger.error('Please determine an appropriate data sheet! ')
            self.SensorInterface_1=MySimulatedIRSensor(sensorDataSheet=self.sensor_file_1,
                                                     position=self.getPositionFromGUI(1),
                                                     orientation=self.getOrientationFromGUI(1),
                                                     logger=logger)
            self.SASensor_1=MySmartAbstractIRSensor(sensor_interface=self.SensorInterface_1,
                                                  outputDataSheet=self.channel_file_0,
                                                  renderer = self.ren,
                                                  appName=[])
            self.SASensor_1.startMeasurements()
            logger.info("SmartAbstractIR Sensor started")
            self.connect(self.SASensor_1,
                         QtCore.SIGNAL("updateGui ( ) "), 
                         self.redraw)       
        self.onStart_common(1)
        
    def onStart_2(self):
        if self.pushButton_start_2.isChecked():
            if self.label_xmlfile_channel_2.text()=='...' or \
               self.label_xmlfile_sensor_2.text()==[]:
                logger.error('Please determine an appropriate data sheet! ')
            self.SensorInterface_2=MySimulatedTemp(sensorDataSheet=self.sensor_file_2,
                                                   logger=[])
            self.SASensor_2=MySmartAbstractTemp(sensorInterface=self.SensorInterface_2,
                                                   eventDataSheet=self.channel_file_2,
                                                   outputfct=[],
                                                   appName=[],
                                                   logger=[])                
        self.onStart_common(2)

    def loadXML_channel_0(self):
        self.loadXML_channel_common(0) 
        
    def loadXML_channel_1(self):
        self.loadXML_channel_common(1)
        
    def loadXML_channel_2(self):
        self.loadXML_channel_common(2)        
        
    def loadXML_channel_common(self, index):
        fd = QtGui.QFileDialog(None)
        channel_file=fd.getOpenFileName()
        exec "self.channel_file_%d=channel_file"%index
        exec "self.label_xmlfile_channel_%d.setText(os.path.basename(str(channel_file)))"%index
        baum = dom.parse(str(channel_file))
        aux = baum.getElementsByTagName('SensorDataSheetFile')
        if aux!=[]:
            content = aux[0].firstChild.data.strip()
            exec "self.label_xmlfile_sensor_%d.setText(os.path.basename(str(content)))"%index
            exec "self.sensor_file_%d=content"%index
        else:            
            self.logger.warning('WARNING - no sensor data sheet')
            exec "self.label_xmlfile_sensor_%d.setText("---")"%index
        logger.info("New data file read for virtual sensor %d"%index)
        
    def setStereoMode(self):
        if self.checkBox_CamStereo.checkState():
            self.vtkWidget.GetRenderWindow().SetStereoRender(1)
        else:
            self.vtkWidget.GetRenderWindow().SetStereoRender(0)
        self.redraw()
        
    def resetCamera(self):
        self.ren.GetActiveCamera().SetPosition(0.0, 0.0, 614.73246763336078)
        self.ren.GetActiveCamera().SetViewUp(0.0, 1.0, 0.0)
        self.ren.GetActiveCamera().SetFocalPoint(0.0, 0.0, 47.5)
        self.redraw()
        
    def standardCamera(self):        
        self.ren.GetActiveCamera().SetPosition(9.0771443932283304, -566.72506794105004, 247.8299526806523)
        self.ren.GetActiveCamera().SetViewUp(-0.0011343377745808704, 0.54348693673594728, 0.83941685882235506)
        self.ren.GetActiveCamera().SetFocalPoint(-3.6197621940835187, -36.197621940833855, 47.5)
        self.redraw()
        
    def redraw(self):
        self.iren.GetRenderWindow().Render()
                
    def robotCamera(self):
        print "robotCam"
        
    def resizeEvent(self,event):
        self.vtkWidget.resize(self.widget_diagram.size())
                
    def init_figures(self):
        self.vtkWidget = QVTKRenderWindowInteractor(self.widget_diagram)
        self.ren = vtk.vtkRenderer()
        self.vtkWidget.GetRenderWindow().AddRenderer(self.ren)
        self.iren = self.vtkWidget.GetRenderWindow().GetInteractor() 
               
        # Plane
        srcPlane = vtk.vtkPlaneSource()
        srcPlane.SetOrigin(-120, 120, -5)
        srcPlane.SetPoint1( 120, 120, -5)
        srcPlane.SetPoint2(-120,  -120, -5)
        srcPlane.SetResolution(10, 10)
        
        mapPlane = vtk.vtkPolyDataMapper();
        mapPlane.SetInput( srcPlane.GetOutput() )
        
        actPlane = vtk.vtkActor()
        actPlane.SetMapper(mapPlane)
        
        # Circle
        srcCircle = vtk.vtkRegularPolygonSource()
        srcCircle.SetRadius(100)
        srcCircle.SetNumberOfSides(50)
        srcCircle.GeneratePolygonOff()
        
        mapCircle = vtk.vtkPolyDataMapper();
        mapCircle.SetInput( srcCircle.GetOutput() )
        
        actCircle = vtk.vtkActor()
        actCircle.SetMapper(mapCircle)
        actCircle.SetPosition(0,0,0.1)
        actCircle.GetProperty().SetColor(1,0,0)
        
        #Robot
        srcRobot = vtk.vtkSTLReader();
        srcRobot.SetFileName("resources/robot.stl");
        
        mapRobot = vtk.vtkPolyDataMapper();
        mapRobot.SetInput(srcRobot.GetOutput())
        
        self.actRobot = vtk.vtkActor()
        self.actRobot.SetMapper(mapRobot)
        self.actRobot.SetPosition(0,100,0)
        self.actRobot.SetOrientation(0, 0, 90)
        self.actRobot.SetScale(6,6,6)
        self.actRobot.GetProperty().SetColor(0.5, 0.5, 0.5)
        #self.actRobot.GetProperty().SetColor(red)      

        # Andre
        # Create a vtkCubeAxesActor2D.  Use the outer edges of the bounding box to
        # draw the axes.  Add the actor to the renderer.
        axes = vtk.vtkCubeAxesActor2D()
        axes.SetCamera(self.ren.GetActiveCamera())
        axes.SetLabelFormat("%6.4g")
        axes.SetFlyModeToOuterEdges()
        axes.SetFontFactor(1)
        axes.SetBounds(-120, 120, -120, 120, 0, 100)
        self.ren.AddViewProp(axes)
        # erdnA
        self.ren.SetBackground( 0.6, 0.6, 0.8 )
        self.ren.AddActor( actPlane  )
        self.ren.AddActor( actCircle )
        self.ren.AddActor( self.actRobot  )
                
        axesActor = vtk.vtkAxesActor();
        self.axes = vtk.vtkOrientationMarkerWidget()
        self.axes.SetOrientationMarker(axesActor)
        self.axes.SetInteractor(self.iren)
        self.axes.EnabledOn()
        self.axes.InteractiveOn()
        #self.ren.ResetCamera()
        self.ren.GetActiveCamera().SetPosition(9.0771443932283304, -566.72506794105004, 247.8299526806523)
        self.ren.GetActiveCamera().SetViewUp(-0.0011343377745808704, 0.54348693673594728, 0.83941685882235506)
        self.ren.GetActiveCamera().SetFocalPoint(-3.6197621940835187, -36.197621940833855, 47.5)       

        self.iren.Start()  
        
    def onStop(self):
        if hasattr(self, 'SimTimer'):
            self.SimTimer.stop()
        if hasattr(self, 'SensorInterface_0'):
            self.SensorInterface_0.stopTimer()
        if hasattr(self, 'SensorInterface_1'):
            self.SensorInterface_1.stopTimer()
        if hasattr(self, 'SensorInterface_2'):
            self.SensorInterface_2.stopTimer()
        print "Aus Maus"     
        sys.exit(1)

if __name__ == "__main__": 
    app = QtGui.QApplication(sys.argv) 
    dialog = GUI() 
    dialog.show() 

    formatter = logging.Formatter("%(asctime)s - %(levelname)s - %(message)s",
                                  '%H:%M:%S')      
    logger = logging.getLogger("GUI")
    ch = QTextEditLoggingHandler(dialog.textEdit__logging)
    ch.setFormatter(formatter)
    logger.addHandler(ch)
    logger.setLevel(logging.DEBUG)
    logger.info("Start Simulated") 
    
    sys.exit(app.exec_())